/*
 * clocking.cpp
 *
 *  Created on: 20.01.2013
 *      Author: kreyl
 */

#include "kl_lib_f4xx.h"
#include "clocking_F40x.h"
#include "stm32_rcc.h"

// Changed to stm32f4xx by RJ

Clk_t Clk;

// ==== Inner use ====
uint8_t Clk_t::HSEEnable() {
    RCC->CR |= RCC_CR_HSEON;    // Enable HSE
    // Wait until ready
    uint32_t StartUpCounter=0;
    do {
        if(RCC->CR & RCC_CR_HSERDY) return 0;   // HSE is ready
        StartUpCounter++;
    } while(StartUpCounter < HSE_STARTUP_TIMEOUT);
    return 1; // Timeout
}

uint8_t Clk_t::HSIEnable() {
    RCC->CR |= RCC_CR_HSION;
    // Wait until ready
    uint32_t StartUpCounter=0;
    do {
        if(RCC->CR & RCC_CR_HSIRDY) return 0;   // HSE is ready
        StartUpCounter++;
    } while(StartUpCounter < HSE_STARTUP_TIMEOUT);
    return 1; // Timeout
}

uint8_t Clk_t::PLLEnable() {
    RCC->CR |= RCC_CR_PLLON;
    // Wait until ready
    uint32_t StartUpCounter=0;
    do {
        if(RCC->CR & RCC_CR_PLLRDY) return 0;   // PLL is ready
        StartUpCounter++;
    } while(StartUpCounter < HSE_STARTUP_TIMEOUT);
    return 1; // Timeout
}

void Clk_t::UpdateFreqValues() {
    uint32_t tmp, InputDiv_M, Multi_N, SysDiv_P, Div_Q;
    uint32_t SysFreqHz=0, PllFreq=0;     // SYSCLK, 168 MHz max, used for Ethernet PTP clk
    // Tables
    const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
    const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4};
    // Figure out SysClk
    tmp = RCC->CFGR & RCC_CFGR_SWS;
    tmp >>= 2;
    switch(tmp) {
    	case 0b00:	// HSI used
    		SysFreqHz = HSI_FREQ_HZ;
    		break;

    	case 0b01:	// HSE used
    		SysFreqHz = CRYSTAL_FREQ_HZ;
    		break;

    	case 0b10: 	// PLL used
    		// Get PLL System dividers
    		InputDiv_M 	= (RCC->PLLCFGR & RCC_PLLCFGR_PLLM)		 ;
    		Multi_N 	= (RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6 ;
    		SysDiv_P   	= (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >> 16) + 1 ) * 2;
    		Div_Q		= (RCC->PLLCFGR & RCC_PLLCFGR_PLLQ) >> 23;
    		PllFreq 	= (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC_HSE)? CRYSTAL_FREQ_HZ : HSI_VALUE;
    		PllFreq 	= (PllFreq / InputDiv_M) * Multi_N;
            SysFreqHz = PllFreq / SysDiv_P;
    		break;
    } // switch
    // AHB Freq
    tmp = AHBPrescTable[(RCC->CFGR & RCC_CFGR_HPRE) >> 4];
    AHBFreqHz = SysFreqHz >> tmp;
    // APB1 Freq
    tmp = APBPrescTable[(RCC->CFGR & RCC_CFGR_PPRE1) >> 10];
    APB1FreqHz = AHBFreqHz >> tmp;
    // APB2 Freq
    tmp = APBPrescTable[(RCC->CFGR & RCC_CFGR_PPRE2) >> 13];
    APB2FreqHz = AHBFreqHz >> tmp;
    // UsbSdioFreq
    if(Div_Q >= 2) UsbSdioFreqHz = PllFreq / Div_Q;
}

// ==== Common use ====

// AHB, APB1, APB2
// Can be divided only by 2,4,8 or 16 to
//
void Clk_t::SetupBusDividers(AHBDiv_t AHBDiv, APBDiv_t APB1Div, APBDiv_t APB2Div) {
    // Setup dividers
    uint32_t tmp = RCC->CFGR;
    tmp &= ~(RCC_CFGR_HPRE | RCC_CFGR_PPRE1 | RCC_CFGR_PPRE2);  // Clear bits
    tmp |= ((uint32_t)AHBDiv)  << 4;
    tmp |= ((uint32_t)APB1Div) << 10;
    tmp |= ((uint32_t)APB2Div) << 13;
    RCC->CFGR = tmp;
}

// Enables HSI, switches to HSI
uint8_t Clk_t::SwitchToHSI() {
    if(HSIEnable() != 0) return 1;
    uint32_t tmp = RCC->CFGR;
    tmp &= ~RCC_CFGR_SW;      // }
    tmp |=  RCC_CFGR_SW_HSI;  // } Select HSI as system clock src
    RCC->CFGR |= tmp;
    while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_HSI); // Wait till ready
    return 0;
}

// Enables HSE, switches to HSE
uint8_t Clk_t::SwitchToHSE() {
    if(HSEEnable() != 0) return 1;
    uint32_t tmp = RCC->CFGR;
    tmp &= ~RCC_CFGR_SW;      // }
    tmp |=  RCC_CFGR_SW_HSE;  // } Select HSE as system clock src
    RCC->CFGR |= tmp;
    while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_HSE); // Wait till ready
    return 0;
}

// Enables HSE, enables PLL, switches to PLL
uint8_t Clk_t::SwitchToPLL() {
    if(HSEEnable() != 0) return 1;
    if(PLLEnable() != 0) return 2;
    uint32_t tmp = RCC->CFGR;
    tmp &= ~RCC_CFGR_SW;          // }
    tmp |=  RCC_CFGR_SW_PLL;      // } Select PLL as system clock src
    RCC->CFGR |= tmp;
    while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL); // Wait until ready
    return 0;
}

// Disable PLL first!
// InputDiv_M: 2...63;  Multi_N:  192...432;
// SysDiv_P: sd2,4,6,8; UsbDiv_Q: 2...15.
uint8_t Clk_t::SetupPLLDividers(uint8_t InputDiv_M, uint16_t Multi_N, PllSysDiv_P_t SysDiv_P, uint8_t UsbDiv_Q) {
    if(RCC->CR & RCC_CR_PLLON) return 1;    // PLL must be disabled to change dividers
    uint32_t tmp = RCC->PLLCFGR;
    tmp =
            RCC_PLLCFGR_PLLSRC_HSE |        // Use only HSE as src
            ((uint32_t)InputDiv_M << 0) |
            ((uint32_t)Multi_N  << 6) |
            ((uint32_t)SysDiv_P << 16) |
            ((uint32_t)UsbDiv_Q << 24);
    RCC->PLLCFGR = tmp;
    return 0;
}

// Setup Flash latency depending on CPU freq and voltage. Page 62 of ref manual.
void Clk_t::SetupFlashLatency(uint8_t AHBClk_MHz) {
    uint32_t tmp = FLASH_ACR_PRFTEN | FLASH_ACR_ICEN | FLASH_ACR_DCEN;
    switch (Voltage.Vcc) {
    	case Vcc1V8_2V1:
			if     (AHBClk_MHz <= 20) tmp |= FLASH_ACR_LATENCY_0WS;     //  0..20
			else if(AHBClk_MHz <= 40) tmp |= FLASH_ACR_LATENCY_1WS;     //  20..40
			else if(AHBClk_MHz <= 60) tmp |= FLASH_ACR_LATENCY_2WS;     //  40..60
			else if(AHBClk_MHz <= 80) tmp |= FLASH_ACR_LATENCY_3WS;     //  60..80
			else if(AHBClk_MHz <=100) tmp |= FLASH_ACR_LATENCY_4WS;     //  80..100
			else if(AHBClk_MHz <=120) tmp |= FLASH_ACR_LATENCY_5WS;     //  100..120
			else if(AHBClk_MHz <=140) tmp |= FLASH_ACR_LATENCY_6WS;     //  120..140
			else                      tmp |= FLASH_ACR_LATENCY_7WS;     //  140..168
    		break;
        case Vcc2V1_2V4:
			if     (AHBClk_MHz <= 22) tmp |= FLASH_ACR_LATENCY_0WS;     //  0..22
			else if(AHBClk_MHz <= 44) tmp |= FLASH_ACR_LATENCY_1WS;     //  22..44
			else if(AHBClk_MHz <= 66) tmp |= FLASH_ACR_LATENCY_2WS;     //  44..66
			else if(AHBClk_MHz <= 88) tmp |= FLASH_ACR_LATENCY_3WS;     //  66..88
			else if(AHBClk_MHz <=110) tmp |= FLASH_ACR_LATENCY_4WS;     //  88..110
			else if(AHBClk_MHz <=132) tmp |= FLASH_ACR_LATENCY_5WS;     //  110..132
			else if(AHBClk_MHz <=154) tmp |= FLASH_ACR_LATENCY_6WS;     //  132..154
			else                      tmp |= FLASH_ACR_LATENCY_7WS;     //  154..168
            break;
        case Vcc2V4_2V7:
			if     (AHBClk_MHz <= 24) tmp |= FLASH_ACR_LATENCY_0WS;     //  0..24
			else if(AHBClk_MHz <= 48) tmp |= FLASH_ACR_LATENCY_1WS;     //  24..48
			else if(AHBClk_MHz <= 72) tmp |= FLASH_ACR_LATENCY_2WS;     //  48..72
			else if(AHBClk_MHz <= 96) tmp |= FLASH_ACR_LATENCY_3WS;     //  72..96
			else if(AHBClk_MHz <=120) tmp |= FLASH_ACR_LATENCY_4WS;     //  96..120
			else if(AHBClk_MHz <=144) tmp |= FLASH_ACR_LATENCY_5WS;     //  120..144
			else                      tmp |= FLASH_ACR_LATENCY_6WS;     //  144..168
            break;
        case Vcc2V7_3V6:
        default:
			if     (AHBClk_MHz <= 30) tmp |= FLASH_ACR_LATENCY_0WS;     // 0..30
			else if(AHBClk_MHz <= 60) tmp |= FLASH_ACR_LATENCY_1WS;     // 30..60
			else if(AHBClk_MHz <= 90) tmp |= FLASH_ACR_LATENCY_2WS;     // 60..90
			else if(AHBClk_MHz <=120) tmp |= FLASH_ACR_LATENCY_3WS;     // 90..120
			else if(AHBClk_MHz <=150) tmp |= FLASH_ACR_LATENCY_4WS;     // 120..150
			else                      tmp |= FLASH_ACR_LATENCY_5WS;     // 150..168
            break;
    }
    FLASH->ACR = (uint32_t)(0x0000FFFF | tmp);
}


// =============================== V Core ======================================
/*  Setup VCore register
 * This bit controls the main internal voltage regulator output voltage to
 * achieve a trade-off between performance and power consumption when the
 * device does not operate at the maximum frequency
*/

VoltageReg_t Voltage;

void VoltageReg_t::SetupVCore(uint32_t AHBValue) {
    // PWR clock enable
    RCC->APB1ENR = RCC_APB1ENR_PWREN;
    // Core voltage setup
    uint32_t tmp = PWR->CR;
    tmp &= ~PWR_CR_VOS;
    if(AHBValue == 168000000) return;
    else tmp |= PWR_CR_VOS;
    PWR->CR = tmp;
    while((PWR->CSR & PWR_CSR_VOSRDY) !=0);
}

/*
 * Early initialization code.
 * This initialization must be performed just after stack setup and before
 * any other initialization.
 */
//void __early_init(void) {
//    RCC->APB1ENR = RCC_APB1ENR_PWREN;   // PWR clock enable
//    PWR->CR = 0;                        // PWR initialization
//
//    // Enable HSI. It is enabled by default, but who knows.
//    RCC->CR |= RCC_CR_HSION;
//    while(!(RCC->CR & RCC_CR_HSIRDY));
//
//    // SYSCFG clock enabled here because it is a multi-functional unit
//    // shared among multiple drivers using external IRQs
//    rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, 1);
//}
